#include <time.h>
#include <assert.h>
#include "tyn_coder.h"

const int iS16_STRATEGY32[15][32] = {
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2},
{4, 4, 4, 4, 4, 4, 4, 4},
{6, 6, 6, 7, 7},
{8, 8, 8, 8},
{8, 16, 8},
{8, 8, 16},
{12, 10, 10},
{12, 12, 8},
{14, 10, 8},
{16, 8, 8},
{16, 16},
{24, 8},
{8, 24},
{32}
};
const int iS16_STRATEGY64[15][64] = {
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2},
{4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4},
{6, 6, 6, 6, 6, 6, 6, 6, 8, 8},
{8, 8, 8, 8, 8, 8, 8, 8},
{9, 9, 9, 9, 9, 9, 10},
{10, 10, 10 ,10, 12, 12},
{12, 12, 12, 14, 14},
{16, 16, 16, 16},
{20, 22, 22},
{20, 24, 20},
{16, 48},
{48, 16},
{32, 32},
{64}
};
//here we use an array to store the capacity of each bit-strategy
const int iS16_CAPACITY32[15] = {32, 16, 8, 5, 4, 3, 3, 3, 3, 3, 3, 2, 2, 2, 1};
const int iS16_CAPACITY64[15] = {64, 32, 16, 10, 8, 7, 6, 5, 4, 3, 3, 2, 2, 2, 1};

const size_t block_length_table[] = {
    32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 
    32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 32, 
    64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 
    64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 
    128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 
    128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 
    128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 
    128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 
};

inline size_t count_bits32(uint32_t value) {
    if (value == 0) {
        return 1;
    }
    size_t l = 1;
    if (value >> 16 > 0) { value >>= 16; l += 16; }
    if (value >> 8 > 0) { value >>= 8; l += 8; }
    if (value >> 4 > 0) { value >>= 4; l += 4; }
    if (value >> 2 > 0) { value >>= 2; l += 2; }
    if (value >> 1 > 0) { value >>= 1; l += 1; }
    return l;
}

inline size_t count_bits64(uint64_t value) {
    if (value == 0) {
        return 1;
    }
    size_t l = 1;
    if (value >> 32 > 0) { value >>= 32; l += 32; }
    if (value >> 16 > 0) { value >>= 16; l += 16; }
    if (value >> 8 > 0) { value >>= 8; l += 8; }
    if (value >> 4 > 0) { value >>= 4; l += 4; }
    if (value >> 2 > 0) { value >>= 2; l += 2; }
    if (value >> 1 > 0) { value >>= 1; l += 1; }
    return l;
}

size_t tyn_iS16_encode32(uint32_t *in, size_t cnt_in, uint8_t *out) {
        int i, j;
        size_t blk_cnt = 0;
        int  cnt_left = cnt_in;
        uint8_t bits;

        //allocate the buffer for bit-strategy storage
        uint32_t blk_buf_int;
        uint32_t *blk_buf = &blk_buf_int;

        while(cnt_left > 0){
                //iterate the bit-strategies to find out the best one
                for(i = 0; i < 15; i++) {
                        j = 0;
                        //assert(*(in + j) > 0);
                        while(j < cnt_left && j < iS16_CAPACITY32[i] && *(in + j) <= (0xffffffff >> (32 - iS16_STRATEGY32[i][j]))) {
                                j++;
                        }
                        if( j == iS16_CAPACITY32[i]){
                                //we've found the best so quit the loop
                                break;
                        }
                }
                assert(j>0);
                //iS16_STRATEGY32[i] is the best one
                //assemble the block(4 bytes)
                *blk_buf = 0;
                for(j = 0, bits = 0; j < iS16_CAPACITY32[i]; j++) {
                        *blk_buf |=  *(in + j) << bits;
                        bits += iS16_STRATEGY32[i][j];
                }

                blk_cnt++;
                if(blk_cnt % 2) {
                        *out = i << 4;
                        memcpy(out+1, blk_buf, 4);
                } else {
                        *out |= i;
                        memcpy(out+5, blk_buf, 4);
                        out += 9;
                }
                cnt_left -= j;
                in += j;
        }
        
        //post-process, add termination suffix
        if(blk_cnt % 2){
                *out |= 0xf;
        } else {
                *out = 0xff;

        }
        return (blk_cnt % 2 ? (blk_cnt+1) / 2 : blk_cnt / 2 + 1) + blk_cnt * 4;
}

size_t tyn_iS16_encode64(uint64_t *in, size_t cnt_in, uint8_t *out) {
        int i, j;
        size_t blk_cnt = 0;
        int  cnt_left = cnt_in;
        uint8_t bits;

        //allocate the buffer for bit-strategy storage
        uint64_t blk_buf_int;
        uint64_t *blk_buf = &blk_buf_int;

        while(cnt_left > 0){
                //iterate the bit-strategies to find out the best one
                for(i = 0; i < 15; i++) {
                        j = 0;
                        //assert(*(in + j) > 0);
                        while(j < cnt_left && j < iS16_CAPACITY64[i] && *(in + j) <= (0xffffffffffffffff >> (64 - iS16_STRATEGY64[i][j]))) {
                                j++;
                        }
                        if( j == iS16_CAPACITY64[i]){
                                //we've found the best so quit the loop
                                break;
                        }
                }
                assert(j>0);
                //iS16_STRATEGY32[i] is the best one
                //assemble the block(4 bytes)
                *blk_buf = 0;
                for(j = 0, bits = 0; j < iS16_CAPACITY64[i]; j++) {
                        *blk_buf |=  *(in + j) << bits;
                        bits += iS16_STRATEGY64[i][j];
                }

                blk_cnt++;
                if(blk_cnt % 2) {
                        *out = i << 4;
                        memcpy(out+1, blk_buf, 8);
                } else {
                        *out |= i;
                        memcpy(out+9, blk_buf, 8);
                        out += 17;
                }
                cnt_left -= j;
                in += j;
        }
        
        //post-process, add termination suffix
        if(blk_cnt % 2){
                *out |= 0xf;
        } else {
                *out = 0xff;

        }
        return (blk_cnt % 2 ? (blk_cnt+1) / 2 : blk_cnt / 2 + 1) + blk_cnt * 8;
}

size_t tyn_iS16_decode32(uint8_t *in, uint32_t *out, size_t *bytes_read) {
        size_t cnt_decoded = 0;
        uint8_t bs; //bs for bit-strategy
        uint32_t *cur;
        int i;
        int t =0;

        *bytes_read = 0;
        while(1) {
                bs = *in >> 4;
                (*bytes_read)++;
                cur = in+1;
                for(i = 0; i < 2; i++) {
                        switch(bs) {
                                case 0:
                                        //{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
                                        *out++ = *cur & 0x1;
                                        *out++ = (*cur & 0x2) >> 1;
                                        *out++ = (*cur & 0x4) >> 2;
                                        *out++ = (*cur & 0x8) >> 3;
                                        *out++ = (*cur & 0x10) >> 4;
                                        *out++ = (*cur & 0x20) >> 5;
                                        *out++ = (*cur & 0x40) >> 6;
                                        *out++ = (*cur & 0x80) >> 7;
                                        *out++ = (*cur & 0x100) >> 8;
                                        *out++ = (*cur & 0x200) >> 9;
                                        *out++ = (*cur & 0x400) >> 10;
                                        *out++ = (*cur & 0x800) >> 11;
                                        *out++ = (*cur & 0x1000) >> 12;
                                        *out++ = (*cur & 0x2000) >> 13;
                                        *out++ = (*cur & 0x4000) >> 14;
                                        *out++ = (*cur & 0x8000) >> 15;
                                        *out++ = (*cur & 0x10000) >> 16;
                                        *out++ = (*cur & 0x20000) >> 17;
                                        *out++ = (*cur & 0x40000) >> 18;
                                        *out++ = (*cur & 0x80000) >> 19;
                                        *out++ = (*cur & 0x100000) >> 20;
                                        *out++ = (*cur & 0x200000) >> 21;
                                        *out++ = (*cur & 0x400000) >> 22;
                                        *out++ = (*cur & 0x800000) >> 23;
                                        *out++ = (*cur & 0x1000000) >> 24;
                                        *out++ = (*cur & 0x2000000) >> 25;
                                        *out++ = (*cur & 0x4000000) >> 26;
                                        *out++ = (*cur & 0x8000000) >> 27;
                                        *out++ = (*cur & 0x10000000) >> 28;
                                        *out++ = (*cur & 0x20000000) >> 29;
                                        *out++ = (*cur & 0x40000000) >> 30;
                                        *out++ = (*cur & 0x80000000) >> 31;
                                        cnt_decoded += 32;
                                break;
                                case 1:
                                        //{2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2},
                                        *out++ = *cur & 0x3;
                                        *out++ = (*cur & 0xc) >> 2;
                                        *out++ = (*cur & 0x30) >> 4;
                                        *out++ = (*cur & 0xc0) >> 6;
                                        *out++ = (*cur & 0x300) >> 8;
                                        *out++ = (*cur & 0xc00) >> 10;
                                        *out++ = (*cur & 0x3000) >> 12;
                                        *out++ = (*cur & 0xc000) >> 14;
                                        *out++ = (*cur & 0x30000) >> 16;
                                        *out++ = (*cur & 0xc0000) >> 18;
                                        *out++ = (*cur & 0x300000) >> 20;
                                        *out++ = (*cur & 0xc00000) >> 22;
                                        *out++ = (*cur & 0x3000000) >> 24;
                                        *out++ = (*cur & 0xc000000) >> 26;
                                        *out++ = (*cur & 0x30000000) >> 28;
                                        *out++ = (*cur & 0xc0000000) >> 30;
                                        cnt_decoded += 16;
                                break;
                                case 2:
                                        //{4, 4, 4, 4, 4, 4, 4, 4},
                                        *out++ = *cur & 0xf;
                                        *out++ = (*cur & 0xf0) >> 4;
                                        *out++ = (*cur & 0xf00) >> 8;
                                        *out++ = (*cur & 0xf000) >> 12;
                                        *out++ = (*cur & 0xf0000) >> 16;
                                        *out++ = (*cur & 0xf00000) >> 20;
                                        *out++ = (*cur & 0xf000000) >> 24;
                                        *out++ = (*cur & 0xf0000000) >> 28;
                                        cnt_decoded += 8;
                                break;
                                case 3:
                                        //{6, 6, 6, 5, 5, 4}
                                        *out++ = *cur & 0x3f;
                                        *out++ = (*cur & 0xfc0) >> 6;
                                        *out++ = (*cur & 0x3f000) >> 12;
                                        *out++ = (*cur & 0x1fc0000) >> 18;
                                        *out++ = (*cur & 0xfe000000) >> 25;
                                        cnt_decoded += 5;
                                break;  
                                case 4:
                                        //{8, 8, 8, 8},
                                        *out++ = *cur & 0xff;
                                        *out++ = (*cur & 0xff00) >> 8;
                                        *out++ = (*cur & 0xff0000) >> 16;
                                        *out++ = (*cur & 0xff000000) >> 24;
                                        cnt_decoded += 4;
                                break;
                                case 5:
                                        //{8, 16, 8}
                                        *out++ = *cur & 0xff;
                                        *out++ = (*cur & 0xffff00) >> 8;
                                        *out++ = (*cur & 0xff000000) >> 24;
                                        cnt_decoded += 3;
                                break;
                                case 6:
                                        //{8, 8, 16}
                                        *out++ = *cur & 0xff;
                                        *out++ = (*cur & 0xff00) >> 8;
                                        *out++ = (*cur & 0xffff0000) >> 16;
                                        cnt_decoded += 3;
                                break;
                                case 7:
                                        //{12, 10, 10}
                                        *out++ = *cur & 0xfff;
                                        *out++ = (*cur & 0x3ff000) >> 12;
                                        *out++ = (*cur & 0xffc00000) >> 22;
                                        cnt_decoded += 3;
                                break;
                                case 8:
                                        //{12, 12, 8}
                                        *out++ = *cur & 0xfff;
                                        *out++ = (*cur & 0xfff000) >> 12;
                                        *out++ = (*cur & 0xff000000) >> 24;
                                        cnt_decoded += 3;
                                break;
                                case 9:
                                        //{14, 10, 8}
                                        *out++ = *cur & 0x3fff;
                                        *out++ = (*cur & 0xffc000) >> 14;
                                        *out++ = (*cur & 0xff000000) >> 24;
                                        cnt_decoded += 3;
                                break;
                                case 10:
                                        //{16, 8, 8}
                                        *out++ = *cur & 0xffff;
                                        *out++ = (*cur & 0xff0000) >> 16;
                                        *out++ = (*cur & 0xff000000) >> 24;
                                        cnt_decoded += 3;
                                break;
                                case 11:
                                        //{16, 16}
                                        *out++ = *cur & 0xffff;
                                        *out++ = (*cur & 0xffff0000) >> 16;
                                        cnt_decoded += 2;
                                break;
                                case 12:
                                        //{24, 8}
                                        *out++ = *cur & 0xffffff;
                                        *out++ = (*cur & 0xff000000) >> 24;
                                        cnt_decoded += 2;
                                break;
                                case 13:
                                        //{8, 24}
                                        *out++ = *cur & 0xff;
                                        *out++ = (*cur & 0xffffff00) >> 8;
                                        cnt_decoded += 2;
                                break;
                                case 14:
                                        //{32}
                                        *out++ = *cur;
                                        cnt_decoded += 1;
                                break;
                                case 15:
                                        //termintion
                                        return cnt_decoded;
                                break;
                                default:
                                        printf("iS16 decode exception!");
                                        exit(1);
                                break;

                        }
                        *bytes_read += 4;
                        bs = *in & 0xf;
                        cur++;
                }
                in = cur;       
        }
}
size_t tyn_iS16_decode64(uint8_t *in, uint64_t *out, size_t *bytes_read) {
        size_t cnt_decoded = 0;
        uint8_t bs; //bs for bit-strategy
        uint64_t *cur;
        int i;
        int t =0;

        *bytes_read = 0;
        while(1) {
                bs = *in >> 4;
                (*bytes_read)++;
                cur = in+1;
                for(i = 0; i < 2; i++) {
                        switch(bs) {
                                case 0:
                                        //{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
                                        *out++ = *cur & 0x1;
                                        *out++ = (*cur & 0x2) >> 1;
                                        *out++ = (*cur & 0x4) >> 2;
                                        *out++ = (*cur & 0x8) >> 3;
                                        *out++ = (*cur & 0x10) >> 4;
                                        *out++ = (*cur & 0x20) >> 5;
                                        *out++ = (*cur & 0x40) >> 6;
                                        *out++ = (*cur & 0x80) >> 7;
                                        *out++ = (*cur & 0x100) >> 8;
                                        *out++ = (*cur & 0x200) >> 9;
                                        *out++ = (*cur & 0x400) >> 10;
                                        *out++ = (*cur & 0x800) >> 11;
                                        *out++ = (*cur & 0x1000) >> 12;
                                        *out++ = (*cur & 0x2000) >> 13;
                                        *out++ = (*cur & 0x4000) >> 14;
                                        *out++ = (*cur & 0x8000) >> 15;
                                        *out++ = (*cur & 0x10000) >> 16;
                                        *out++ = (*cur & 0x20000) >> 17;
                                        *out++ = (*cur & 0x40000) >> 18;
                                        *out++ = (*cur & 0x80000) >> 19;
                                        *out++ = (*cur & 0x100000) >> 20;
                                        *out++ = (*cur & 0x200000) >> 21;
                                        *out++ = (*cur & 0x400000) >> 22;
                                        *out++ = (*cur & 0x800000) >> 23;
                                        *out++ = (*cur & 0x1000000) >> 24;
                                        *out++ = (*cur & 0x2000000) >> 25;
                                        *out++ = (*cur & 0x4000000) >> 26;
                                        *out++ = (*cur & 0x8000000) >> 27;
                                        *out++ = (*cur & 0x10000000) >> 28;
                                        *out++ = (*cur & 0x20000000) >> 29;
                                        *out++ = (*cur & 0x40000000) >> 30;
                                        *out++ = (*cur & 0x80000000) >> 31;
                                        *out++ = (*cur & 0x100000000) >> 32;
                                        *out++ = (*cur & 0x200000000) >> 33;
                                        *out++ = (*cur & 0x400000000) >> 34;
                                        *out++ = (*cur & 0x800000000) >> 35;
                                        *out++ = (*cur & 0x1000000000) >> 36;
                                        *out++ = (*cur & 0x2000000000) >> 37;
                                        *out++ = (*cur & 0x4000000000) >> 38;
                                        *out++ = (*cur & 0x8000000000) >> 39;
                                        *out++ = (*cur & 0x10000000000) >> 40;
                                        *out++ = (*cur & 0x20000000000) >> 41;
                                        *out++ = (*cur & 0x40000000000) >> 42;
                                        *out++ = (*cur & 0x80000000000) >> 43;
                                        *out++ = (*cur & 0x100000000000) >> 44;
                                        *out++ = (*cur & 0x200000000000) >> 45;
                                        *out++ = (*cur & 0x400000000000) >> 46;
                                        *out++ = (*cur & 0x800000000000) >> 47;
                                        *out++ = (*cur & 0x1000000000000) >> 48;
                                        *out++ = (*cur & 0x2000000000000) >> 49;
                                        *out++ = (*cur & 0x4000000000000) >> 50;
                                        *out++ = (*cur & 0x8000000000000) >> 51;
                                        *out++ = (*cur & 0x10000000000000) >> 52;
                                        *out++ = (*cur & 0x20000000000000) >> 53;
                                        *out++ = (*cur & 0x40000000000000) >> 54;
                                        *out++ = (*cur & 0x80000000000000) >> 55;
                                        *out++ = (*cur & 0x100000000000000) >> 56;
                                        *out++ = (*cur & 0x200000000000000) >> 57;
                                        *out++ = (*cur & 0x400000000000000) >> 58;
                                        *out++ = (*cur & 0x800000000000000) >> 59;
                                        *out++ = (*cur & 0x1000000000000000) >> 60;
                                        *out++ = (*cur & 0x2000000000000000) >> 61;
                                        *out++ = (*cur & 0x4000000000000000) >> 62;
                                        *out++ = (*cur & 0x8000000000000000) >> 63;
                                        cnt_decoded += 64;
                                break;
                                case 1:
                                        //{2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2},
                                        *out++ = *cur & 0x3;
                                        *out++ = (*cur & 0xc) >> 2;
                                        *out++ = (*cur & 0x30) >> 4;
                                        *out++ = (*cur & 0xc0) >> 6;
                                        *out++ = (*cur & 0x300) >> 8;
                                        *out++ = (*cur & 0xc00) >> 10;
                                        *out++ = (*cur & 0x3000) >> 12;
                                        *out++ = (*cur & 0xc000) >> 14;
                                        *out++ = (*cur & 0x30000) >> 16;
                                        *out++ = (*cur & 0xc0000) >> 18;
                                        *out++ = (*cur & 0x300000) >> 20;
                                        *out++ = (*cur & 0xc00000) >> 22;
                                        *out++ = (*cur & 0x3000000) >> 24;
                                        *out++ = (*cur & 0xc000000) >> 26;
                                        *out++ = (*cur & 0x30000000) >> 28;
                                        *out++ = (*cur & 0xc0000000) >> 30;
                                        *out++ = (*cur & 0x300000000) >> 32;
                                        *out++ = (*cur & 0xc00000000) >> 34;
                                        *out++ = (*cur & 0x3000000000) >> 36;
                                        *out++ = (*cur & 0xc000000000) >> 38;
                                        *out++ = (*cur & 0x30000000000) >> 40;
                                        *out++ = (*cur & 0xc0000000000) >> 42;
                                        *out++ = (*cur & 0x300000000000) >> 44;
                                        *out++ = (*cur & 0xc00000000000) >> 46;
                                        *out++ = (*cur & 0x3000000000000) >> 48;
                                        *out++ = (*cur & 0xc000000000000) >> 50;
                                        *out++ = (*cur & 0x30000000000000) >> 52;
                                        *out++ = (*cur & 0xc0000000000000) >> 54;
                                        *out++ = (*cur & 0x300000000000000) >> 56;
                                        *out++ = (*cur & 0xc00000000000000) >> 58;
                                        *out++ = (*cur & 0x3000000000000000) >> 60;
                                        *out++ = (*cur & 0xc000000000000000) >> 62;
                                        cnt_decoded += 32;
                                break;
                                case 2:
                                        //{4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4},
                                        *out++ = *cur & 0xf;
                                        *out++ = (*cur & 0xf0) >> 4;
                                        *out++ = (*cur & 0xf00) >> 8;
                                        *out++ = (*cur & 0xf000) >> 12;
                                        *out++ = (*cur & 0xf0000) >> 16;
                                        *out++ = (*cur & 0xf00000) >> 20;
                                        *out++ = (*cur & 0xf000000) >> 24;
                                        *out++ = (*cur & 0xf0000000) >> 28;
                                        *out++ = (*cur & 0xf00000000) >> 32;
                                        *out++ = (*cur & 0xf000000000) >> 36;
                                        *out++ = (*cur & 0xf0000000000) >> 40;
                                        *out++ = (*cur & 0xf00000000000) >> 44;
                                        *out++ = (*cur & 0xf000000000000) >> 48;
                                        *out++ = (*cur & 0xf0000000000000) >> 52;
                                        *out++ = (*cur & 0xf00000000000000) >> 56;
                                        *out++ = (*cur & 0xf000000000000000) >> 60;
                                        cnt_decoded += 16;
                                break;
                                case 3:
                                        //{6, 6, 6, 6, 6, 6, 6, 6, 8, 8},
                                        *out++ = *cur & 0x3f;
                                        *out++ = (*cur & 0xfc0) >> 6;
                                        *out++ = (*cur & 0x3f000) >> 12;
                                        *out++ = (*cur & 0xfc0000) >> 18;
                                        *out++ = (*cur & 0x3f000000) >> 24;
                                        *out++ = (*cur & 0xfc0000000) >> 30;
                                        *out++ = (*cur & 0x3f000000000) >> 36;
                                        *out++ = (*cur & 0xfc0000000000) >> 42;
                                        *out++ = (*cur & 0xff000000000000) >> 48;
                                        *out++ = (*cur & 0xff00000000000000) >> 56;

                                        cnt_decoded += 10;
                                break;  
                                case 4:
                                        //{8, 8, 8, 8, 8, 8, 8, 8},
                                        *out++ = *cur & 0xff;
                                        *out++ = (*cur & 0xff00) >> 8;
                                        *out++ = (*cur & 0xff0000) >> 16;
                                        *out++ = (*cur & 0xff000000) >> 24;
                                        *out++ = (*cur & 0xff00000000) >> 32;
                                        *out++ = (*cur & 0xff0000000000) >> 40;
                                        *out++ = (*cur & 0xff000000000000) >> 48;
                                        *out++ = (*cur & 0xff00000000000000) >> 56;
                                        cnt_decoded += 8;
                                break;
                                case 5:
                                        //{9, 9, 9, 9, 9, 9, 10},
                                        *out++ = *cur & 0x1ff;
                                        *out++ = (*cur & 0x3fe00) >> 9;
                                        *out++ = (*cur & 0x7fc0000) >> 18;
                                        *out++ = (*cur & 0xff8000000) >> 27;
                                        *out++ = (*cur & 0x1ff000000000) >> 36;
                                        *out++ = (*cur & 0x3fe00000000000) >> 45;
                                        *out++ = (*cur & 0xffc0000000000000) >> 54;
                                        cnt_decoded += 7;
                                break;
                                case 6:
                                        //{10, 10, 10 ,10, 12, 12},
                                        *out++ = *cur & 0x3ff;
                                        *out++ = (*cur & 0xffc00) >> 10;
                                        *out++ = (*cur & 0x3ff00000) >> 20;
                                        *out++ = (*cur & 0xffc0000000) >> 30;
                                        *out++ = (*cur & 0xfff0000000000) >> 40;
                                        *out++ = (*cur & 0xfff0000000000000) >> 52;
                                        cnt_decoded += 6;
                                break;
                                case 7:
                                        //{12, 12, 12, 14, 14},
                                        *out++ = *cur & 0xfff;
                                        *out++ = (*cur & 0xfff000) >> 12;
                                        *out++ = (*cur & 0xfff000000) >> 24;
                                        *out++ = (*cur & 0x3fff000000000) >> 36;
                                        *out++ = (*cur & 0xfffc000000000000) >> 50;
                                        cnt_decoded += 5;
                                break;
                                case 8:
                                        //{16, 16, 16, 16},
                                        *out++ = *cur & 0xffff;
                                        *out++ = (*cur & 0xffff0000) >> 16;
                                        *out++ = (*cur & 0xffff00000000) >> 32;
                                        *out++ = (*cur & 0xffff000000000000) >> 48;
                                        cnt_decoded += 4;
                                break;
                                case 9:
                                        //{20, 22, 22},
                                        *out++ = *cur & 0xfffff;
                                        *out++ = (*cur & 0x3fffff00000) >> 20;
                                        *out++ = (*cur & 0xfffffc0000000000) >> 42;
                                        cnt_decoded += 3;
                                break;
                                case 10:
                                        //{20, 24, 20},
                                        *out++ = *cur & 0xfffff;
                                        *out++ = (*cur & 0xffffff00000) >> 20;
                                        *out++ = (*cur & 0xfffff00000000000) >> 44;
                                        cnt_decoded += 3;
                                break;
                                case 11:
                                        //{16, 48},
                                        *out++ = *cur & 0xffff;
                                        *out++ = (*cur & 0xffffffffffff0000) >> 16;
                                        cnt_decoded += 2;
                                break;
                                case 12:
                                        //{48, 16},
                                        *out++ = *cur & 0xffffffffffff;
                                        *out++ = (*cur & 0xffff000000000000) >> 48;
                                        cnt_decoded += 2;
                                break;
                                case 13:
                                        //{32, 32},
                                        *out++ = *cur & 0xffffffff;
                                        *out++ = (*cur & 0xffffffff00000000) >> 32;
                                        cnt_decoded += 2;
                                break;
                                case 14:
                                        //{64}
                                        *out++ = *cur;
                                        cnt_decoded += 1;
                                break;
                                case 15:
                                        //termintion
                                        return cnt_decoded;
                                break;
                                default:
                                        printf("iS16 decode exception!");
                                        exit(1);
                                break;

                        }
                        *bytes_read += 8;
                        bs = *in & 0xf;
                        cur++;
                }
                in = cur;       
        }
}

size_t tyn_p4d_encode32(uint32_t *in, size_t cnt_in, uint8_t *out, int add_termination) {
        size_t cnt_left = cnt_in;
        int length_stat[32];
        
        size_t encoded_size = 0;
         
        while(cnt_left > 0) {
            if(cnt_left > 127 ) {
                //get block size
                size_t blk_size = 128;
                uint8_t num_items = blk_size;
                //initialize the bit-size statistics table
                memset(length_stat, 0, sizeof(int) * 32);
                //scan this block
                for( int i = 0; i < num_items; i++) {
                        length_stat[count_bits32(*(in + i)) - 1]++;     
                }
                //find the best bit-size
                //we use stat to speed up compression
                size_t threshold = (cnt_left < blk_size ? cnt_left : blk_size ) * 0.5;
                size_t p = 0;
                uint8_t best_bit = 0;
                for(int i = 0; i< 32; i++) {
                        p += length_stat[i];
                        if( p > threshold) {
                                best_bit = i;
                                break;
                        }
                }
                //here best_bit is a number we found approrite to start with to reduce the compression time
                size_t best_block_size = 1024000; //just big enough for comparison
                size_t best_num_exp;
                for(int i = best_bit; i< 32; i++) {
                        if(length_stat[i]) {
                                uint8_t bit_size = i + 1;
                                uint8_t num_exp = 0;
                                uint32_t mask = 0xffffffff >> (32 - bit_size);
                                for(int i = 0; i < num_items; i++) {
                                        if( *(in + i) > mask ) {
                                                num_exp++;
                                        }
                                }
                                //header_size = 3 + num_exp * ( 4 + 1);
                                size_t tmp_size = 3 + num_exp * (4 + 1) + blk_size * bit_size / 8;
                                if( tmp_size < best_block_size ) {
                                        best_block_size = tmp_size;
                                        best_bit = bit_size;
                                        best_num_exp = num_exp;
                                }
                        }//end if (length_stat[i])
                }//end for(i = 0; i< 32; i++)
                //now we've got the best bit
                //assert(best_bit > 0);
                
                uint8_t *pos_ptr = out + 3;
                uint32_t *exp_ptr = out + 3 + best_num_exp;
                uint32_t *out_ptr = out + 3 + best_num_exp * 5;
                
                uint32_t mask = 0xffffffff >> (32 - best_bit);
                *out_ptr = 0;
                for(int i = 0; i < num_items; i++) {
                        size_t start_pos = (i * best_bit) / 32 ;
                        size_t start_bit = (i * best_bit) % 32;

                        if( *in > mask ) {
                                //this is an exception
                                *pos_ptr++ = i;
                                *exp_ptr++ = *in >> best_bit;

                                //fill the bits
                                *(out_ptr + start_pos) |= (*in & mask) << start_bit;
                                if( start_bit + best_bit > 31 ) {
                                        *(out_ptr + start_pos + 1) = (*in & mask) >> ( 32 - start_bit);
                                }
                        } else {
                                //fill the bits
                                *(out_ptr + start_pos) |= *in << start_bit;
                                if( start_bit + best_bit > 31 ) {
                                        *(out_ptr + start_pos +1) = *in >> ( 32 - start_bit);
                                }
                        }
                        in++;
                }

                *out = num_items;
                *(out + 1) = best_num_exp;
                *(out + 2) = best_bit;
                
                cnt_left -= num_items;
                //size_t block_size = best_header_size + blk_size * best_bit / 8;
                encoded_size += best_block_size;
                out += best_block_size; 
            } else {
                //cnt_left <= 127
                *out++ = (uint8_t)cnt_left;
                size_t size = tyn_iS16_encode32(in, cnt_left, out);
                encoded_size += size + 1;
                out += size;
                cnt_left = 0;
            }
        }

        if(add_termination) {
            *out = 0;
            return encoded_size + 1;
        } else {
            return encoded_size;
        }
        
}
size_t tyn_p4d_encode64(uint64_t *in, size_t cnt_in, uint8_t *out, int add_termination) {
        size_t cnt_left = cnt_in;
        int length_stat[64];
        
        size_t encoded_size = 0;
         
        while(cnt_left > 0) {
            if(cnt_left > 127 ) {
                //get block size
                size_t blk_size = 128;
                uint8_t num_items = blk_size;
                //initialize the bit-size statistics table
                memset(length_stat, 0, sizeof(int) * 64);
                //scan this block
                for( int i = 0; i < num_items; i++) {
                        length_stat[count_bits64(*(in + i)) - 1]++;     
                }
                //find the best bit-size
                //we use stat to speed up compression
                size_t threshold = (cnt_left < blk_size ? cnt_left : blk_size ) * 0.5;
                size_t p = 0;
                uint8_t best_bit = 0;
                for(int i = 0; i< 64; i++) {
                        p += length_stat[i];
                        if( p > threshold) {
                                best_bit = i;
                                break;
                        }
                }
                //here best_bit is a number we found approrite to start with to reduce the compression time
                size_t best_block_size = 1024000; //just big enough for comparison
                size_t best_num_exp;
                for(int i = best_bit; i< 64; i++) {
                        if(length_stat[i]) {
                                uint8_t bit_size = i + 1;
                                uint8_t num_exp = 0;
                                uint64_t mask = 0xffffffffffffffff >> (64 - bit_size);
                                for(int i = 0; i < num_items; i++) {
                                        if( *(in + i) > mask ) {
                                                num_exp++;
                                        }
                                }
                                //header_size = 3 + num_exp * ( 4 + 1);
                                size_t tmp_size = 3 + num_exp * (8 + 1) + blk_size * bit_size / 8;
                                if( tmp_size < best_block_size ) {
                                        best_block_size = tmp_size;
                                        best_bit = bit_size;
                                        best_num_exp = num_exp;
                                }
                        }//end if (length_stat[i])
                }//end for(i = 0; i< 32; i++)
                //now we've got the best bit
                //assert(best_bit > 0);
                
                uint8_t *pos_ptr = out + 3;
                uint64_t *exp_ptr = out + 3 + best_num_exp;
                uint64_t *out_ptr = out + 3 + best_num_exp * 9;
                
                uint64_t mask = 0xffffffffffffffff >> (64 - best_bit);
                *out_ptr = 0;
                for(int i = 0; i < num_items; i++) {
                        size_t start_pos = (i * best_bit) / 64 ;
                        size_t start_bit = (i * best_bit) % 64;

                        if( *in > mask ) {
                                //this is an exception
                                *pos_ptr++ = i;
                                *exp_ptr++ = *in >> best_bit;

                                //fill the bits
                                *(out_ptr + start_pos) |= (*in & mask) << start_bit;
                                if( start_bit + best_bit > 63 ) {
                                        *(out_ptr + start_pos + 1) = (*in & mask) >> ( 64 - start_bit);
                                }
                        } else {
                                //fill the bits
                                *(out_ptr + start_pos) |= *in << start_bit;
                                if( start_bit + best_bit > 63 ) {
                                        *(out_ptr + start_pos +1) = *in >> ( 64 - start_bit);
                                }
                        }
                        in++;
                }

                *out = num_items;
                *(out + 1) = best_num_exp;
                *(out + 2) = best_bit;
                
                cnt_left -= num_items;
                //size_t block_size = best_header_size + blk_size * best_bit / 8;
                encoded_size += best_block_size;
                out += best_block_size; 
            } else {
                //cnt_left <= 127
                *out++ = (uint8_t)cnt_left;
                size_t size = tyn_iS16_encode64(in, cnt_left, out);
                encoded_size += size + 1;
                out += size;
                cnt_left = 0;
            }
        }

        if(add_termination) {
            *out = 0;
            return encoded_size + 1;
        } else {
            return encoded_size;
        }
        
}

size_t tyn_p4d_decode32(uint8_t *in, uint32_t *out, size_t *bytes_read) {
        size_t cnt_decoded = 0;
        uint8_t *in_orig = in;
        while(1) {
            uint8_t num_items = *in++;
            if( !num_items ) {
                // a termination tag
                *bytes_read = in - in_orig;
                return cnt_decoded;
            }
            if(num_items == 128 ) {  

                uint8_t num_exp = *in++;
                uint8_t best_bit = *in++;

                uint8_t *pos_ptr = in;
                uint32_t *exp_ptr = in + num_exp;
                uint32_t *in_ptr = in + num_exp * 5;
                uint32_t *out_ptr = out;

                //size_t blk_size = block_length_table[num_items - 1];
                size_t blk_size = 128;

                uint32_t mask = 0xffffffff >> (32 - best_bit);
                int bits = 0;
                for(int i = 0; i < num_items; i++) {
                        //size_t start_pos = bits / 32 ;
                        int start_pos = bits >> 5 ;
                        int start_bit = bits % 32;
                
                        *out_ptr = *(in_ptr + start_pos) >> start_bit;
                        *out_ptr &= mask;
                        if( start_bit + best_bit > 32 ) {
                                *out_ptr |= ((*(in_ptr + start_pos + 1) << (32 - start_bit))) & mask;
                        }
                        out_ptr++;
                        bits+=best_bit;
                }
                if(num_exp) {
                    for(int i = 0; i < num_exp; i++) {
                            *(out + *(pos_ptr + i)) |= *(exp_ptr + i) << best_bit;
                    }
                }
                cnt_decoded += num_items;
                out += num_items;
                in += num_exp * 5 + blk_size * best_bit / 8;
            } else {
                //num_items <= 127
                size_t bytes_read;
                size_t size;
                size = tyn_iS16_decode32(in, out, &bytes_read);
                cnt_decoded += size;
                in += bytes_read;
                
            }
        }
}

size_t tyn_p4d_decode64(uint8_t *in, uint64_t *out, size_t *bytes_read) {
        size_t cnt_decoded = 0;
        uint8_t *in_orig = in;
        while(1) {
            uint8_t num_items = *in++;
            if( !num_items ) {
                // a termination tag
                *bytes_read = in - in_orig;
                return cnt_decoded;
            }
            if(num_items == 128 ) {  

                uint8_t num_exp = *in++;
                uint8_t best_bit = *in++;

                uint8_t *pos_ptr = in;
                uint64_t *exp_ptr = in + num_exp;
                uint64_t *in_ptr = in + num_exp * 9;
                uint64_t *out_ptr = out;

                //size_t blk_size = block_length_table[num_items - 1];
                size_t blk_size = 128;

                uint64_t mask = 0xffffffffffffffff >> (64 - best_bit);
                int bits = 0;
                for(int i = 0; i < num_items; i++) {
                        //size_t start_pos = bits / 32 ;
                        int start_pos = bits >> 6 ;
                        int start_bit = bits % 64;
                
                        *out_ptr = *(in_ptr + start_pos) >> start_bit;
                        *out_ptr &= mask;
                        if( start_bit + best_bit > 64 ) {
                                *out_ptr |= ((*(in_ptr + start_pos + 1) << (64 - start_bit))) & mask;
                        }
                        out_ptr++;
                        bits+=best_bit;
                }
                if(num_exp) {
                    for(int i = 0; i < num_exp; i++) {
                            *(out + *(pos_ptr + i)) |= *(exp_ptr + i) << best_bit;
                    }
                }
                cnt_decoded += num_items;
                out += num_items;
                in += num_exp * 9 + blk_size * best_bit / 8;
            } else {
                //num_items <= 127
                size_t bytes_read;
                size_t size;
                size = tyn_iS16_decode64(in, out, &bytes_read);
                cnt_decoded += size;
                in += bytes_read;
                
            }
        }
}

